module ackerman

use param
use random
use matrix

implicit none

real(8) :: don_cf  = 582000
real(8) :: don_cr  = 783000    
real(8) :: don_cphi= 457000   
real(8) :: don_dphi= 100000   
real(8) :: don_g   = 9.81     
real(8) :: don_hr  = 0.68      
real(8) :: don_h   = 1.15     
real(8) :: don_j2x = 24201 
real(8) :: don_jz  = 34917 
real(8) :: don_lf  = 1.95  
real(8) :: don_lr  = 1.54  
real(8) :: don_m   = 14300 
real(8) :: don_m2  = 12487 
real(8) :: don_mu  = 1.0   
real(8) :: don_tt  = 1.86  
real(8) :: don_v   = 17.0

real(8) :: don_eps = 2.5*10.0**(-2)
real(8) :: don_sigma = 0.5
real(8) :: don_nu = 3.09

contains

subroutine simuAck(time_tab,LTR)
real(8), dimension(time_N+1) :: LTR ,time_tab

integer :: i, errorinv
real(8), dimension(8,8) :: mat_A,mat_eye8
real(8), dimension(8,1)  :: vec_diff,vec_temp
real(8), dimension(1,8)  :: vec_X0
real(8), dimension(3,3)   :: mat_M, mat_invM, mat_D, mat_G, mat_L
real(8), dimension(3,1)     :: mat_S, vec_MS
real(8), dimension(1,2)   :: mat_Gamma
real(8), dimension(time_N+1,8)   :: X,Xd
real(8), dimension(time_N+1)   :: incrbrow
real(8) :: time_h, ti, sigma

vec_X0 = 0.0

!sigma = 2.5*10.0**(-3)
sigma = 1.0

time_h = real(time_T)/real(time_N)
time_tab = 0.0
X = 0.0
X(1:1,:) = vec_X0

mat_eye8 = 0.0
do i=1,8
   mat_eye8(i,i) = 1.0
end do

mat_M = 0.0
mat_M(1,1) = don_m
mat_M(1,3) = -don_h*don_m2
mat_M(2,2) = don_jz
mat_M(3,1) = mat_M(1,3)
mat_M(3,3) = don_j2x + (don_h**2)*don_m2

mat_D = 0.0
mat_D(1,1) = (don_cf+don_cr)*don_mu/don_v;
mat_D(1,2) = (don_cf*don_lf-don_cr*don_lr)*don_mu/don_v + don_m*don_v/2.0;
mat_D(2,1) = mat_D(1,2);
mat_D(2,2) = (don_cf*(don_lf**2)+don_cr*(don_lr**2))*don_mu/don_v;
mat_D(2,3) = -0.5*don_h*don_m2*don_v;
mat_D(3,2) = mat_D(2,3);
mat_D(3,3) = don_dphi;

mat_G = 0.0
mat_G(1,2) = 0.5*don_m*don_v;
mat_G(2,1) = -mat_G(1,2);
mat_G(2,3) = 0.5*don_h*don_m2*don_v;
mat_G(3,2) = -mat_G(2,3);

mat_L = 0.0
mat_L(3,3) = don_cphi - don_m2*don_g*don_h;

mat_S = 0.0
mat_S(1,1) = don_cf*don_mu;
mat_S(2,1) = don_cf*don_lf*don_mu;

mat_Gamma = 0.0
mat_Gamma(1,1) = sqrt(4.0*don_nu**2+don_sigma**4)
mat_Gamma(1,2) = 2.0
mat_Gamma = don_eps*don_sigma*sqrt(2.0)*mat_Gamma

call invmatrix(mat_M, mat_invM, 3, errorinv)

mat_A = 0.0
mat_A(1,4) = -1.0
mat_A(2,5) = -1.0
mat_A(3,6) = -1.0

mat_A(4:6,1:3) = matmul(mat_invM,mat_L) 
mat_A(4:6,4:6) = matmul(mat_invM,mat_G+mat_D)
mat_A(4:6,7:8) = -matmul(mat_invM,matmul(mat_S,mat_Gamma))

mat_A(7:8,1:6) = 0.0
mat_A(7,7:8) = (/0.0, -1.0 /)
mat_A(8,7:8) = (/don_nu**2.0+0.25*don_sigma**4.0,don_sigma**2.0 /)

vec_MS = matmul(mat_invM,mat_S)

vec_diff = 0.0
vec_diff(8,1) = 0.25

vec_temp = 0.0
LTR = 0

incrbrow(1) = sqrt(time_h)*random_normal()

do i=2,time_N+1
   ti = time_h*real(i-1)
   time_tab(i) = ti
   incrbrow(i) = sqrt(time_h)*random_normal()
   vec_temp(4:6,1:1) = time_h*delta(ti)*vec_MS
   X(i:i,:) = transpose(matmul(mat_eye8-time_h*mat_A,transpose(X(i-1:i-1,:))) + vec_temp + incrbrow(i-1)*vec_diff)
   Xd(i:i,:) = -matmul(X(i:i,:),transpose(mat_A)) + transpose(vec_diff)*incrbrow(i) + transpose(vec_temp)
   LTR(i) = 2.0*don_m2/(don_m*don_tt)*((don_hr+don_h)*(Xd(i,4)+don_v*X(i,5)-don_h*Xd(i,6))/don_g+don_h*X(i,3))
end do

LTR = LTR/sigma
end subroutine simuAck

!======================================

subroutine simuAckD(time_tab,LTR)
real(8), dimension(time_N+1)   :: LTR ,time_tab

integer :: i, errorinv
real(8), dimension(11,11) :: mat_A,mat_eye11
real(8), dimension(11,1)  :: vec_diff,vec_temp
real(8), dimension(1,11)  :: vec_X0
real(8), dimension(3,3)   :: mat_M, mat_invM, mat_D, mat_G, mat_L
real(8), dimension(3,1)   :: mat_S, vec_MS
real(8), dimension(1,2)   :: mat_Lambda
real(8), dimension(time_N+1,11)   :: Xd
real(8), dimension(time_N+1)   :: incrbrow
real(8) :: time_h, ti, sigma

sigma = 2.5*10.0**(-3)
sigma = 1.0
vec_X0 = 0.0
!vec_X0(1,4) = don_v

time_h = real(time_T)/real(time_N)

time_tab = 0.0

Xd = 0.0
Xd(1:1,:) = vec_X0

mat_eye11 = 0.0
do i=1,11
   mat_eye11(i,i) = 1.0
end do

mat_M = 0.0
mat_M(1,1) = don_m
mat_M(1,3) = -don_h*don_m2
mat_M(2,2) = don_jz
mat_M(3,1) = mat_M(1,3)
mat_M(3,3) = don_j2x + (don_h**2)*don_m2

mat_D = 0.0
mat_D(1,1) = (don_cf+don_cr)*don_mu/don_v;
mat_D(1,2) = (don_cf*don_lf-don_cr*don_lr)*don_mu/don_v + don_m*don_v/2.0;
mat_D(2,1) = mat_D(1,2);
mat_D(2,2) = (don_cf*(don_lf**2)+don_cr*(don_lr**2))*don_mu/don_v;
mat_D(2,3) = -0.5*don_h*don_m2*don_v;
mat_D(3,2) = mat_D(2,3);
mat_D(3,3) = don_dphi;

mat_G = 0.0
mat_G(1,2) = 0.5*don_m*don_v;
mat_G(2,1) = -mat_G(1,2);
mat_G(2,3) = 0.5*don_h*don_m2*don_v;
mat_G(3,2) = -mat_G(2,3);

mat_L = 0.0
mat_L(3,3) = don_cphi - don_m2*don_g*don_h;

mat_S = 0.0
mat_S(1,1) = don_cf*don_mu;
mat_S(2,1) = don_cf*don_lf*don_mu;

mat_Lambda = 0.0
mat_Lambda(1,1) = -2.0*(don_nu**2+0.25*don_sigma**4);
mat_Lambda(1,2) = sqrt(4.0*don_nu**2+don_sigma**4)-2.0*don_sigma**2;
mat_Lambda = don_eps*don_sigma*sqrt(2.0)*mat_Lambda;

call invmatrix(mat_M, mat_invM, 3, errorinv)

mat_A = 0.0
mat_A(1:3,1:3) = 0.0
mat_A(1,4) = -1.0
mat_A(2,5) = -1.0
mat_A(3,6) = -1.0
mat_A(1:3,7:11) = 0

mat_A(4:6,1:6) = 0.0
mat_A(4,7) = -1.0
mat_A(5,8) = -1.0
mat_A(6,9) = -1.0
mat_A(4:6,10:11) = 0.0

mat_A(7:9,1:3) = 0.0
mat_A(7:9,4:6) = matmul(mat_invM,mat_L) 
mat_A(7:9,7:9) = matmul(mat_invM,mat_G+mat_D)
mat_A(7:9,10:11) = -matmul(mat_invM,matmul(mat_S,mat_Lambda))

mat_A(10:11,1:9) = 0.0
mat_A(10,10:11) = (/0.0, -1.0 /)

mat_A(11,10:11) = (/don_nu**2.0+0.25*don_sigma**4.0,don_sigma**2.0 /)

vec_MS = matmul(mat_invM,mat_S)

vec_diff = 0.0
vec_diff(7:9,1:1) = 0.5*don_eps*don_sigma*sqrt(2.0)*vec_MS
vec_diff(11,1) = 0.25

vec_temp = 0.0

!LTR(1) = 2.0*don_m2/(don_m*don_tt)*((don_hr+don_h)*(Xd(1,7)+don_v*Xd(1,5)-don_h*Xd(1,9))/don_g+don_h*Xd(1,3))
!print*,time_tab(1),Xd(1,1:11),LTR(1)
incrbrow(1) = sqrt(time_h)*random_normal();
do i=2,time_N+1
   ti = time_h*real(i-1)
   time_tab(i) = ti 
   incrbrow(i) = sqrt(time_h)*random_normal()
   vec_temp(7:9,1:1) = time_h*delta(ti)*vec_MS
   Xd(i:i,:) = transpose(matmul(mat_eye11-time_h*mat_A,transpose(Xd(i-1:i-1,:))) + vec_temp + incrbrow(i-1)*vec_diff)
   LTR(i) = 2.0*don_m2/(don_m*don_tt)*((don_hr+don_h)*(Xd(i,7)+don_v*Xd(i,5)-don_h*Xd(i,9))/don_g+don_h*Xd(i,3))
end do

LTR = LTR/sigma

end subroutine simuAckD

function delta(t) result(res)
real(8), intent(in)  :: t
real(8) :: res

if ((t<=2.0).or.(t>15.0)) then
   res = 0.0
elseif ((t>2.0).and.(t<=5.0)) then
   res = 4.0/3.0*(t-2.0)
elseif ((t>5.0).and.(t<=12.0)) then
   res = 4.0
elseif ((t>12.0).and.(t<=15.0)) then
   res = -4.0/3.0*(t-15.0)
end if
res = real(braq_opt)*res*pi/180.0
end function delta

end module ackerman
